/*倒立摆系统*/
#ifndef PENDULUM_H
#define PENDULUM_H
#include "simucpp.hpp"
using namespace simucpp;
using namespace std;

/**********************
简单的单倒立摆
**********************/
class InversePendulum: public PackModule {
public:
    InversePendulum() {};
    InversePendulum(Simulator *sim) {
        Initialize(sim);
    };
    virtual PUnitModule Get_InputPort(uint32_t n) const override {
        if (n==0) return mioF;
        return nullptr;
    };
    virtual PUnitModule Get_OutputPort(uint32_t n) const override {
        if (n==0) return mintx;
        if (n==1) return mintv;
        if (n==2) return minttheta;
        if (n==3) return mintomega;
        return nullptr;
    };
    void Initialize(Simulator *sim){
        SUIntegrator(minttheta, sim);
        SUIntegrator(mintomega, sim);
        SUIntegrator(mintx, sim);
        SUIntegrator(mintv, sim);
        SUFcn(mfcn1, sim);
        SUFcnMISO(mfcn2, sim);
        SUFcn(mfcn3, sim);
        SUFcnMISO(mfcn4, sim);
        SUFcnMISO(mfcn5, sim);
        SUGain(mioF, sim);
        sim->connectU(mfcn5, mintomega); // ω'=n5
        sim->connectU(mintomega, minttheta);  // θ'=ω
        sim->connectU(mfcn4, mintv);  // v'=n4
        sim->connectU(mintv, mintx);  // x'=v
        sim->connectU(minttheta, mfcn1);
        sim->connectU(mintomega, mfcn2);
        sim->connectU(minttheta, mfcn2);
        sim->connectU(minttheta, mfcn3);
        sim->connectU(mfcn1, mfcn4);
        sim->connectU(mioF, mfcn4);
        sim->connectU(mfcn2, mfcn4);
        sim->connectU(mfcn3, mfcn4);
        sim->connectU(mfcn4, mfcn5);
        sim->connectU(minttheta, mfcn5);
        mfcn1->Set_Function([this](double u){return 1/(ipM+ipm*sin(u)*sin(u));});
        mfcn2->Set_Function([this](double *u){return -ipm*ipl*u[0]*u[0]*sin(u[1]);});
        mfcn3->Set_Function([this](double u){return 5*ipm*sin(u+u);});
        mfcn4->Set_Function([this](double *u){return u[0]*(u[1]+u[2]+u[3]);});
        mfcn5->Set_Function([this](double *u){return (u[0]*cos(u[1])+10*sin(u[1]))/ipl;});
        minttheta->Set_InitialValue(0.2);
    }
private:
    double ipM=0.1, ipm=0.5, ipl=0.3;
    UIntegrator *minttheta=nullptr;  // θ
    UIntegrator *mintomega=nullptr;  // ω
    UIntegrator *mintx=nullptr;  // x
    UIntegrator *mintv=nullptr;  // v
    UFcn *mfcn1=nullptr;  // 式n1
    UFcnMISO *mfcn2=nullptr;  // 式n2
    UFcn *mfcn3=nullptr;  // 式n3
    UFcnMISO *mfcn4=nullptr;  // 式n4
    UFcnMISO *mfcn5=nullptr;  // 式n5
    UGain *mioF=nullptr;  // 输入的力F
};


/**********************
<Robust Adaptive Control>中的双倒立摆
**********************//*
class InversePendulumTwo: public PackModule
{
public:
    InversePendulumTwo() {};
    InversePendulumTwo(Simulator *sim) {
        Initialize(sim);
    };
    virtual PUnitModule Get_InputPort(int n) const {
        if (n==0) return mioF;
        return nullptr;
    };
    virtual PUnitModule Get_OutputPort(int n) const {
        if (n==0) return minttheta1;
        if (n==1) return mintomega1;
        if (n==2) return minttheta2;
        if (n==3) return mintomega2;
        if (n==4) return mintv;
        if (n==5) return mintx;
        return nullptr;
    };
    void Initialize(Simulator *sim){
        n1a = new Funcn1a(ipM, ipm1, ipm2);
        n4a = new Funcn4a;
        n12 = new Funcn2(ipm1, ipl1);
        n13 = new Funcn3(ipm1);
        n15 = new Funcn5(ipl1);
        n22 = new Funcn2(ipm2, ipl2);
        n23 = new Funcn3(ipm2);
        n25 = new Funcn5(ipl2);
        SMIntegrator(minttheta1, sim);
        SMIntegrator(mintomega1, sim);
        SMIntegrator(minttheta2, sim);
        SMIntegrator(mintomega2, sim);
        SMIntegrator(mintx, sim);
        SMIntegrator(mintv, sim);
        SMFcnMISO(mfcn1a, sim);
        SMFcnMISO(mfcn4a, sim);
        SMFcnMISO(mfcn12, sim);
        SMFcn(mfcn13, sim);
        SMFcnMISO(mfcn15, sim);
        SMFcnMISO(mfcn22, sim);
        SMFcn(mfcn23, sim);
        SMFcnMISO(mfcn25, sim);
        SMConnector(mioF, sim);
        sim->connect(mfcn15, mintomega1); // ω'=n5
        sim->connect(mintomega1, minttheta1);  // θ'=ω
        sim->connect(mfcn25, mintomega2); // ω'=n5
        sim->connect(mintomega2, minttheta2);  // θ'=ω
        sim->connect(mfcn4a, mintv);  // v'=n4
        sim->connect(mintv, mintx);  // x'=v
        sim->connect(minttheta1, mfcn1a);
        sim->connect(minttheta2, mfcn1a);
        sim->connect(mintomega1, mfcn12);
        sim->connect(minttheta1, mfcn12);
        sim->connect(mintomega2, mfcn22);
        sim->connect(minttheta2, mfcn22);
        sim->connect(minttheta1, mfcn13);
        sim->connect(minttheta2, mfcn23);
        sim->connect(mfcn1a, mfcn4a);
        sim->connect(mioF, mfcn4a);
        sim->connect(mfcn12, mfcn4a);
        sim->connect(mfcn13, mfcn4a);
        sim->connect(mfcn22, mfcn4a);
        sim->connect(mfcn23, mfcn4a);
        sim->connect(mfcn4a, mfcn15);
        sim->connect(mfcn4a, mfcn25);
        sim->connect(minttheta1, mfcn15);
        sim->connect(minttheta2, mfcn25);
        mfcn1a->Set_Function(n1a);
        mfcn4a->Set_Function(n4a);
        mfcn12->Set_Function(n12);
        mfcn13->Set_Function(n13);
        mfcn15->Set_Function(n15);
        mfcn22->Set_Function(n22);
        mfcn23->Set_Function(n23);
        mfcn25->Set_Function(n25);
        minttheta1->Set_InitialValue(0.2);
        minttheta2->Set_InitialValue(0.1);
    }
private:
    double ipM=10, ipm1=1, ipl1=1, ipm2=1, ipl2=2;
    MIntegrator *minttheta1=nullptr;  // θ1
    MIntegrator *mintomega1=nullptr;  // ω1
    MIntegrator *minttheta2=nullptr;  // θ2
    MIntegrator *mintomega2=nullptr;  // ω2
    MIntegrator *mintx=nullptr;  // x
    MIntegrator *mintv=nullptr;  // v
    MFcnMISO *mfcn1a=nullptr;
    MFcnMISO *mfcn4a=nullptr;
    MFcnMISO *mfcn12=nullptr;
    MFcn *mfcn13=nullptr;
    MFcnMISO *mfcn15=nullptr;
    MFcnMISO *mfcn22=nullptr;
    MFcn *mfcn23=nullptr;
    MFcnMISO *mfcn25=nullptr;
    MConnector *mioF=nullptr;
    UserFunc *n1a, *n4a, *n12, *n13, *n15, *n22, *n23, *n25;
};
*/

#endif
